/**
 * @file Armor.h
 * @author enemy1205 (enemy1205@qq.com)
 * @brief 装甲板类头文件
 * @date 2021-08-16
 *
 */
#include "Armor.h"

//#define ARMORDEBUG
using namespace std;
using namespace cv;

/**
 * @brief 构造函数，通过左右灯条初始化
 * @param blobLF 左灯条指针
 * @param blobRT 右灯条指针
 */
Armor::Armor(Blob_ptr &blobLF, Blob_ptr &blobRT) {
    this->blob_rt = blobRT;
    this->blob_lf = blobLF;
    this->height = ((blobLF->getHeight() + blobRT->getHeight()) / 2);
    this->width = getDistance(blobLF->getCenter(), blobRT->getCenter());
//    this->setArmorType();
    //直接中心相加与四顶点相加结果完全相同
//    this->center = ((blobLF->getBpoint() + blobLF->getTpoint() + blobRT->getTpoint() + blobRT->getBpoint()) / 4);
    this->center = (blobLF->getCenter() + blobRT->getCenter()) / 2;
    this->angle = Rad2Deg(getHorizontalAngle(this->blob_rt->getCenter(), this->blob_lf->getCenter()));//angle>0左偏 angle<>>0右偏
    this->isValidPairs();
}

/**
 * @brief 判断两灯条是否能配对
 * @return bool
 */
void Armor::isValidPairs() {
    float angle_diff = abs(blob_lf->getAngle() - blob_rt->getAngle());
//        cout<<"angle  "<<angle_diff<<endl;
    if (angle_diff > A_param.light_max_angle_diff) {
#ifdef ARMORDEBUG
        cout << "灯条角度差不符合要求" << "  angle_diff:" << angle_diff << endl;
#endif
        return;
    }
    //此处发现取灯条高度与取灯条上下顶点差获得高度差别不大
//    cout<<blob_lf->getHeight()<<"  "<<blob_rt->getHeight()<<endl;
//    cout<<(blob_lf->getTpoint().y - blob_lf->getBpoint().y) <<"  "<<(blob_rt->getTpoint().y - blob_rt->getBpoint().y)<<endl;
//        cout<<"height/  "<<(blob_lf->getTpoint().y - blob_lf->getBpoint().y) / (blob_rt->getTpoint().y - blob_rt->getBpoint().y)<<endl;
    float height_ratio = (blob_lf->getHeight() > blob_rt->getHeight()) ? blob_lf->getHeight() / blob_rt->getHeight() :
                         blob_rt->getHeight() / blob_lf->getHeight();
    if (height_ratio > A_param.light_max_height_ratio) {
#ifdef ARMORDEBUG
        cout << "灯条高度比不符合要求" << "  height_ratio:" << height_ratio << endl;
#endif
        return;
    }
    float width_ratio = (blob_lf->getWidth() > blob_rt->getWidth()) ? blob_lf->getWidth() / blob_rt->getWidth() :
                        blob_rt->getWidth() / blob_lf->getWidth();
    if (width_ratio > A_param.light_max_width_ratio) {
#ifdef ARMORDEBUG
        cout << "灯条宽度比不符合要求" << "  width_ratio:" << width_ratio << endl;
#endif
        return;
    }
    /*左右装甲板y轴投影长度是否符合*/
    double up_distance = fabs(blob_lf->getTpoint().y - blob_rt->getTpoint().y);
    double down_distance = fabs(blob_lf->getBpoint().y - blob_rt->getBpoint().y);
    if (down_distance >= A_param.max_up_down_dis_ratio * MIN(blob_rt->getHeight(), blob_lf->getHeight()) ||
        up_distance >= A_param.max_up_down_dis_ratio * MIN(blob_rt->getHeight(), blob_lf->getHeight())) {
#ifdef ARMORDEBUG
        cout << "y轴投影长度不符合要求: " << Point2f(up_distance, down_distance);
        cout << "超出：\t" << A_param.max_up_down_dis_ratio * MIN(blob_rt->getHeight(), blob_lf->getHeight()) << endl;
#endif
        return;
    }
    if (abs(this->angle) > A_param.armor_max_angle) {
#ifdef ARMORDEBUG
        cout << "装甲板角度不符合要求" << "  angle:" << angle << endl;
#endif
        return;
    }
    if (this->wh_ratio() < A_param.armor_min_ratio ||
        this->wh_ratio() > A_param.armor_max_ratio) {
#ifdef ARMORDEBUG
        cout << "装甲板长宽比不符合要求" << "  wh_ratio:" << this->wh_ratio() << endl;
#endif
        return;
    }
    float diagonal = getDistance(this->blob_rt->getTpoint(), this->blob_lf->getBpoint());
    float dig_wid_ratio = acosf(this->width / diagonal);
    if (dig_wid_ratio > A_param.max_dig_width_ratio || dig_wid_ratio < A_param.min_dig_width_ratio) {
#ifdef ARMORDEBUG
        cout << "装甲板斜宽比不符合要求" << "  dig_wid_ratio:" << dig_wid_ratio << endl;
#endif
        return;
    }
    // 更新匹配误差
    this->match_error = angle_diff * A_param.ERROR_ANGLE_DIFF +
                        abs(angle) * A_param.ERROR_ANGLE +
                        abs(height_ratio + width_ratio - 2) * A_param.ERROR_H_W_RATIO +
                        abs(wh_ratio() - 3.368) * A_param.ERROR_WH_RATIO +//2.417,4.264
                        abs(dig_wid_ratio - 0.3844) * A_param.ERROR_DIG_WIDTH;
    this->validity = true;
}
/**
 * @brief 设置装甲板类型
 */
//void Armor::setArmorType() {
//    if (this->wh_ratio() < 3.2) this->armor_type = ArmorType::SMALL;
//    else this->armor_type = ArmorType::BIG;
//}

/**
 * @brief 更新一些在判断时暂且不用的数据,减少运算
 */
void Armor::updateRestInfo() {
    ////                以装甲板实际高度为纵坐标
//    this->vertexes[0] = blob_lf->getBpoint() + Point2f(0, blob_lf->getHeight() / A_param.extend_k);
//    this->vertexes[1] = blob_lf->getTpoint() - Point2f(0, blob_lf->getHeight() / A_param.extend_k);
//    this->vertexes[2] = blob_rt->getTpoint() - Point2f(0, blob_rt->getHeight() / A_param.extend_k);
//    this->vertexes[3] = blob_rt->getBpoint() + Point2f(0, blob_rt->getHeight() / A_param.extend_k);
//                 以灯条高度为纵坐标
    this->vertexes[0] = this->blob_lf->getBpoint();
    this->vertexes[1] = this->blob_lf->getTpoint();
    this->vertexes[2] = this->blob_rt->getTpoint();
    this->vertexes[3] = this->blob_rt->getBpoint();
    this->relative_angles = calculateRelativeAngle(cameraParam.cameraMatrix, cameraParam.distCoeff, this->center);
}


